﻿using System;
using System.Diagnostics.Contracts;
using MathService.Vectors;

namespace MathService
{
    /// <summary>Кватернион</summary>
    public struct Quaternion : IEquatable<Quaternion>, IEquatable<Complex>
    {
        /* -------------------------------------------------------------------------------- */

        public static Quaternion Real { get { return new Quaternion(1, 0, 0, 0); } }
        public static Quaternion i { get { return new Quaternion(0, 1, 0, 0); } }
        public static Quaternion j { get { return new Quaternion(0, 0, 1, 0); } }
        public static Quaternion k { get { return new Quaternion(0, 0, 0, 1); } }

        public static Quaternion Exp(Quaternion q)
        {
            var ExpA = Math.Exp(q.a);
            var v = q.V;
            var absV = v.R;
            return absV.Equals(0)
                ? new Quaternion(ExpA)
                : new Quaternion(ExpA * Math.Cos(absV), v * (ExpA * Math.Sin(absV) / absV));
        }

        public static Quaternion Ln(Quaternion q)
        {
            var a = q.a;
            var v = q.V;
            var absV = v.R;
            var absQ = q.Abs;
            return new Quaternion(Math.Log(absQ), v * (Math.Acos(a / absV) / absV));
        }

        public static Quaternion Sin(Quaternion q)
        {
            var a = q.a;
            var sinA = Math.Sin(a);
            var cosA = Math.Cos(a);

            var v = q.V;
            var absV = v.R;
            var expV = Math.Exp(absV);
            var expVneg = Math.Exp(-absV);

            var chV = .5 * (expV + expVneg);
            var shV = .5 * (expV - expVneg);

            return new Quaternion(sinA * chV, v * (cosA * shV / absV));
        }

        public static Quaternion Cos(Quaternion q)
        {
            var a = q.a;
            var sinA = Math.Sin(a);
            var cosA = Math.Cos(a);

            var v = q.V;
            var absV = v.R;
            var expV = Math.Exp(absV);
            var expVneg = Math.Exp(-absV);

            var chV = .5 * (expV + expVneg);
            var shV = .5 * (expV - expVneg);

            return new Quaternion(cosA * chV, v * (-sinA * shV / absV));
        }

        public static Quaternion Tg(Quaternion q) { return Sin(q) / Cos(q); }

        /* -------------------------------------------------------------------------------- */

        private double a, b, c, d;

        /* -------------------------------------------------------------------------------- */

        public double A { get { return a; } set { a = value; } }
        public double B { get { return b; } set { b = value; } }
        public double C { get { return c; } set { c = value; } }
        public double D { get { return d; } set { d = value; } }

        public double Abs
        {
            get
            {
                Contract.Ensures(Contract.Result<double>() >= 0);
                return Math.Sqrt(a * a + b * b + c * c + d * d);
            }
        }

        public Quaternion Conjugate { get { return new Quaternion(a, -b, -c, -d); } }

        public Quaternion Inversed
        {
            get
            {
                var abs2 = a * a + b * b + c * c + d * d;
                return new Quaternion(a / abs2, -b / abs2, -c / abs2, -d / abs2);
            }
        }

        public bool IsScalar { get { return !a.Equals(0) && b.Equals(0) && c.Equals(0) && d.Equals(0); } }
        public bool IsVector { get { return a.Equals(0) && !b.Equals(0) && !c.Equals(0) && !d.Equals(0); } }

        public Quaternion Part_Scalar { get { return new Quaternion(a, 0, 0, 0); } }
        public Quaternion Part_Vector { get { return new Quaternion(0, b, c, d); } }

        public Vector3D V { get { return new Vector3D(b, c, d); } }

        public Quaternion Sign { get { return this / Abs; } }

        public double Arg
        {
            get
            {
                var a = this.a;
                var abs = Abs;
                return a.Equals(0)
                        ? (abs.Equals(0)                       // Re == 0
                            ? 0                                //  Im == 0 => 0
                            : Math.Sign(abs) * Consts.pi05)    //  Im != 0 => pi/2 * sign(Im)
                        : (abs.Equals(0)                       // Re != 0
                            ? (abs > 0 ? 0 : Consts.pi)
                            : Math.Atan2(abs, a));             //  Im != 0 => atan(Im/Re)
            }
        }

        public Matrix QMatrix
        {
            get
            {
                return new Matrix(new[,]
                    {
                        { a, -b, -c, -d }, 
                        { b, a, -d, c }, 
                        { c, d, a, -b }, 
                        { d, -c, b, a }
                    });
            }
        }

        public MatrixComplex QComplex
        {
            get
            {
                var a11 = new Complex(a, b);
                var a12 = new Complex(c, d);
                var a21 = new Complex(-c, d);
                var a22 = new Complex(a, -b);

                return new MatrixComplex(new[,]
                {
                    { a11, a12 }, 
                    { a21, a22 }
                });
            }
        }

        /* -------------------------------------------------------------------------------- */

        /// <summary>
        /// Кватерион a + b*i + c*j + d*k
        /// </summary>
        /// <param name="a"></param>
        public Quaternion(double a)
        {
            this.a = a;
            b = 0;
            c = 0;
            d = 0;
        }

        /// <summary>
        /// Кватерион a + b*i + c*j + d*k
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <param name="c"></param>
        /// <param name="d"></param>
        public Quaternion(double a, double b, double c, double d)
        {
            this.a = a;
            this.b = b;
            this.c = c;
            this.d = d;
        }

        public Quaternion(Complex z1, Complex z2)
        {
            a = z1.Re;
            b = z1.Im;
            c = z2.Re;
            d = z2.Im;
        }

        public Quaternion(double a, Vector3D v)
        {
            this.a = a;
            b = v.X;
            c = v.Y;
            d = v.Z;
        }

        /* -------------------------------------------------------------------------------- */



        /* -------------------------------------------------------------------------------- */

        public static Quaternion operator +(Quaternion X, Quaternion Y)
        {
            return new Quaternion(X.a + Y.a, X.b + Y.b, X.c + Y.c, X.d + Y.d);
        }

        public static Quaternion operator +(Quaternion X, double c)
        {
            return new Quaternion(X.a + c, X.b + c, X.c + c, X.d + c);
        }

        public static Quaternion operator +(double c, Quaternion X)
        {
            return new Quaternion(X.a + c, X.b + c, X.c + c, X.d + c);
        }

        public static Quaternion operator -(Quaternion X, Quaternion Y)
        {
            return new Quaternion(X.a - Y.a, X.b - Y.b, X.c - Y.c, X.d - Y.d);
        }

        public static Quaternion operator -(Quaternion X, double c)
        {
            return new Quaternion(X.a - c, X.b - c, X.c - c, X.d - c);
        }

        public static Quaternion operator -(double c, Quaternion X)
        {
            return new Quaternion(X.a - c, X.b - c, X.c - c, X.d - c);
        }

        /// <summary>
        /// </summary>
        /// <remarks>
        /// (a1 + b1*i + c1*j + d1*k) * (a2 + b2*i + c2*j + d2*k) =
        /// 
        /// a1*a2 + a1*b2*i + a1*c2*j + a1*d2*k +
        /// b1*i*a2 + b1*i*b2*i + b1*i*c2*j + b1*i*d2*k +  
        /// c1*j*a2 + c1*j*b2*i + c1*j*c2*j + c1*j*d2*k +    
        /// d1*k*a2 + d1*k*b2*i + d1*k*c2*j + d1*k*d2*k =
        /// 
        /// a1*a2   + a1*b2*i + a1*c2*j + a1*d2*k +
        /// b1*a2*i - b1*b2   + b1*c2*k - b1*d2*j +   
        /// c1*a2*j - c1*b2*k - c1*c2   + c1*d2*i + 
        /// d1*a2*k + d1*b2*j - d1*c2*i - d1*d2   =
        /// 
        ///  a1*a2 - b1*b2 - c1*c2 - d1*d2 +       
        /// (a1*b2 + b1*a2 + c1*d2 - d1*c2)*i +    
        /// (a1*c2 + c1*a2 - b1*d2 + d1*b2)*j +
        /// (a1*d2 + d1*a2 + b1*c2 - c1*b2)*k =
        ///                         
        ///  ┌a1 -b1 -c1 -d1┐   ┌a2┐
        ///  │b1  a1 -d1  c1│ x │b2│ = Q1 x q2
        ///  │c1  d1  a1 -b1│   │c2│
        ///  └d1 -c1  b1  a1┘   └d2┘
        /// 
        ///  a1*a2 - b1*b2 - c1*c2 - d1*d2 +       
        /// (b1*a2 + a1*b2 - d1*c2 + c1*d2)*i +    
        /// (c1*a2 + d1*b2 + a1*c2 - b1*d2)*j +
        /// (d1*a2 - c1*b2 + b1*c2 + a1*d2)*k =
        /// 
        ///  ┌a2 -b2 -c2 -d2┐   ┌a1┐
        ///  │b2  a2  d2 -c2│ x │b1│
        ///  │c2 -d2  a2  b2│   │c1│
        ///  └d2  c2 -b2  a2┘   └d1┘
        ///        
        ///  a1*a2 - b1*b2 - c1*c2 - d1*d2 +       
        /// (a1*b2 + b1*a2 + c1*d2 - d1*c2)*i +    
        /// (a1*c2 - b1*d2 + c1*a2 + d1*b2)*j +
        /// (a1*d2 + b1*c2 - c1*b2 + d1*a2)*k
        /// 
        /// x| 1  i  j  k
        /// -+-----------
        /// 1| 1  i  j  k
        /// i| i -1  k -j
        /// j| j -k -1  i
        /// k| k  j -i -1
        /// 
        /// </remarks>
        /// <param name="X"></param>
        /// <param name="Y"></param>
        /// <returns></returns>
        public static Quaternion operator *(Quaternion X, Quaternion Y)
        {
            var a = X.a * Y.a - X.b * Y.b - X.c * Y.c - X.d * Y.d;
            var b = X.a * Y.b + X.b * Y.a + X.c * Y.d - X.d * Y.c;
            var c = X.a * Y.c + X.c * Y.a - X.b * Y.d + X.d * Y.b;
            var d = X.a * Y.d + X.d * Y.a + X.b * Y.c - X.c * Y.b;
            return new Quaternion(a, b, c, d);
        }

        public static Quaternion operator *(Quaternion X, double c)
        {
            return new Quaternion(X.a * c, X.b * c, X.c * c, X.d * c);
        }

        public static Quaternion operator *(double c, Quaternion X)
        {
            return new Quaternion(X.a * c, X.b * c, X.c * c, X.d * c);
        }

        public static Quaternion operator /(Quaternion X, Quaternion Y) { return X * Y.Inversed; }
        public static Quaternion operator /(double c, Quaternion X) { return c * X.Inversed; }
        public static Quaternion operator /(Quaternion X, double c) { return new Quaternion(X.a / c, X.b / c, X.c / c, X.d / c); }

        /* -------------------------------------------------------------------------------- */

        public bool Equals(Quaternion other)
        {
            return
                a.Equals(other.a) &&
                b.Equals(other.b) &&
                c.Equals(other.c) &&
                d.Equals(other.d);
        }

        public bool Equals(Complex other)
        {
            return c.Equals(0) && d.Equals(0) && a.Equals(other.Re) && b.Equals(other.Im);
        }

        public override bool Equals(object obj)
        {
            return !ReferenceEquals(null, obj) &&
                ((obj is Quaternion && Equals((Quaternion)obj)) || (obj is Complex && Equals((Complex)obj)));
        }

        public override int GetHashCode()
        {
            unchecked
            {
                var lv_HashCode = a.GetHashCode();
                lv_HashCode = (lv_HashCode * 397) ^ b.GetHashCode();
                lv_HashCode = (lv_HashCode * 397) ^ c.GetHashCode();
                lv_HashCode = (lv_HashCode * 397) ^ d.GetHashCode();
                return lv_HashCode;
            }
        }

        /* -------------------------------------------------------------------------------- */
    }
}
